/**
  ******************************************************************************
  * @file    actuator.h
  * @author  Mentos Seetoo
  * @brief   This file provides class and definitions about actuators, which is 
  *          used to calculate the real torque at joint.  			
  * @note    - The provided frictions are for joints, and they are optional.
  *          - The R/KT are motor parameters.
  *          - The computational formulas used in this file are basical 
  *            relationships in lectromechanics.           
  * @warning At least C++11 is required.
  *          This file refers to 'Actuator.h' in Cheetah Control code.    														 
  ******************************************************************************
  */
#ifndef ACTUATOR_MODEL_H
#define ACTUATOR_MODEL_H
/*!
 * Constrain in to be between min and max
 */
template <typename T>
T constrain(T in, T min, T max)
{
    if (in < min)
    {
        in = min;
    }
    if (in > max)
    {
        in = max;
    }
    return in;
}

/*!
 * A model of an actuator containing friction and electrical effects
 */
template <typename T>
class ActuatorModel
{
public:
    /*!
    * Construct a new actuator model with the given parameters
    * @param gearRatio : Gear reduction
    * @param motorKT : Value of KT (torque constant) for the motor
    * @param motorR : Motor resistance
    * @param batteryV : Battery voltage
    * @param damping : Actuator damping (at the joint, Nm/(rad/sec))
    * @param dryFriction : Actuator dry friction (at the joint, Nm)
    * @param tauMax : Maximum torque output of the actuator
    */
    ActuatorModel(T gearRatio, T motorKT, T motorR, T batteryV, T damping,
                  T dryFriction, T tauMax)
        : _gr(gearRatio),
          _kt(motorKT),
          _R(motorR),
          _V(batteryV),
          _damping(damping),
          _dryFriction(dryFriction),
          _tauMax(tauMax) {}

    ActuatorModel() {}
    /*!
    * Compute actual actuator torque, given desired torque and speed.
    * takes into account friction (dry and damping), voltage limits, and torque
    * limits
    * @param tauDes : desired torque
    * @param qd : current actuator velocity (at the joint)
    * @return actual produced torque
    */
    T getTorque(T tauDes, T qd)
    {
        // compute motor torque
        T tauDesMotor = tauDes / _gr;          // motor torque
        T iDes = tauDesMotor / (_kt * 1.5);    // i = tau / KT
        //T bemf =  qd * _gr * _kt * 1.732;    // back emf
        T bemf = qd * _gr * _kt * 2.;          // back emf
        T vDes = iDes * _R + bemf;             // v = I*R + emf
        T vActual = constrain(vDes, -_V, _V);  // limit to battery voltage
        T tauActMotor =
            1.5 * _kt * (vActual - bemf) / _R; // tau = Kt * I = Kt * V / R
        T tauAct = _gr * constrain(tauActMotor, -_tauMax, _tauMax);

        // add damping and dry friction
        if (_frictionEnabled)
            tauAct = tauAct - _damping * qd - _dryFriction * sgn(qd);

        return tauAct;
    }

    /*!
        * Control friction effects
        * @param enabled : enable/disable both dry and damping friction terms
        */
    void setFriction(bool enabled) { _frictionEnabled = enabled; }

private:
    bool _frictionEnabled = true;
    T _gr, _kt, _R, _V, _damping, _dryFriction, _tauMax;
};

#endif //ACTUATOR_MODEL_H
/************************ END OF FILE **************************/